
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       Multicopter.c
  * @author     baiyang
  * @date       2021-7-14
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "Multicopter.h"
#include "SitlSensor/Atmtable.h"
#include "uITC/uITC.h"
#include "uITC/uITC_msg.h"

#include <common/time/gp_time.h>
#include <stdlib.h>

#include<WinSock2.h>
#include <winsock.h>

#include <common/console/console.h>

#pragma comment(lib,"ws2_32.lib")
/*-----------------------------------macro------------------------------------*/
#define ISA_GAS_CONSTANT 287.26f
#define C_TO_KELVIN 273.15f


#define YAW_FACTOR_CW   -1
#define YAW_FACTOR_CCW  1

#define MOTORS_MOT_1 0U
#define MOTORS_MOT_2 1U
#define MOTORS_MOT_3 2U
#define MOTORS_MOT_4 3U
#define MOTORS_MOT_5 4U
#define MOTORS_MOT_6 5U
#define MOTORS_MOT_7 6U
#define MOTORS_MOT_8 7U
#define MOTORS_MOT_9 8U
#define MOTORS_MOT_10 9U
#define MOTORS_MOT_11 10U
#define MOTORS_MOT_12 11U

/*----------------------------------typedef-----------------------------------*/
//四轴
static Motor_Frame quad_x[] =
{
    {MOTORS_MOT_1,   45, YAW_FACTOR_CCW, 1},
    {MOTORS_MOT_2, -135, YAW_FACTOR_CCW, 3},
    {MOTORS_MOT_3,  -45, YAW_FACTOR_CW,  4},
    {MOTORS_MOT_4,  135, YAW_FACTOR_CW,  2}
};

//六轴
static Motor_Frame hex_x[] = 
{
    { MOTORS_MOT_1,    0.0f, YAW_FACTOR_CCW, 1 },
    { MOTORS_MOT_2,  180.0f, YAW_FACTOR_CCW, 4 },
    { MOTORS_MOT_3, -120.0f, YAW_FACTOR_CW,  5 },
    { MOTORS_MOT_4,   60.0f, YAW_FACTOR_CW,  2 },
    { MOTORS_MOT_5,  -60.0f, YAW_FACTOR_CW,  6 },
    { MOTORS_MOT_6,  120.0f, YAW_FACTOR_CW,  3 }
};

//默认F450机架
Mode default_mode = {
    .mass = 2.0f,
    .uavR = 0.225f,
    //转动惯量
    .jxx = 0.0211f,
    .jyy = 0.0219f,
    .jzz = 0.0366f,
    //电机相关参数
    .kv = 880.0f,
    .no_load_current = 0.7f,
    .resistance = 0.115f,
    .esc_resistance = 0.0f,
    .max_current = 20.0f,
    .pwmMin = 1000.0f,
    .pwmMax = 2000.0f,
    .spin_min = 0.15f,
    .spin_max = 0.95f,

    //电池参数
    .max_voltage = 12.6f,
    .batt_resistance = 0.0034f,
    .capacity = 5.2f,
    //螺旋桨参数
    .diameter = 0.245f,            //m
    .pitch = 0.1143f,            //m
    .prop_mass = 0.0125f,
    .Cm = 1.13f,            //扭矩系数
    .Ct = 1.0f,            //拉力系数
    .intertia = 0.0001287f, //电机转子和螺旋桨的总转动惯量

    //环境参数
    .TempC = 25.0f,
    .Damp_Cd = 0.055f,    //N/(m/s)2
    .Damp_CCm = {0.0035f, 0.0039f, 0.0034f}
};

const uint32_t FG_NET_FDM_VERSION = 24;

Frame MultiCopter;

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void Multi_LandRun(void);
void SimpleAtmosphere(const float alt, float* sigma, float* delta, float* theta);
static float get_air_density(float alt, float tempc);

void MultiCopter_Init()
{
    float start_yaw = 0.0f;

    Location start_loc;
    location_zero(&start_loc);

    start_loc.lat = (int32_t)(37.62873 * 1e7);
    start_loc.lng = (int32_t)(-122.39339 * 1e7);
    start_loc.alt = 50;

    MultiCopter.name = NULL;
    
    //使用默认模型
    MultiCopter.mode = default_mode;
    Aircraft_SetStartLoc(&(MultiCopter.air_craft), start_loc, start_yaw);
    //初始化电池
    Battery_Setup(&(MultiCopter.battery), MultiCopter.mode.capacity, MultiCopter.mode.batt_resistance, MultiCopter.mode.max_voltage);
    Battery_InitVoltage(&(MultiCopter.battery), MultiCopter.mode.max_voltage);
    //初始化地面模型
    Land_Init(&(MultiCopter.land));
    //使用默认四轴
    MultiCopter.motor_frame = quad_x;
    MultiCopter.num_motors = 4;
    MultiCopter.pMotor = (Motor *)malloc(sizeof(Motor)*MultiCopter.num_motors);
    memset(MultiCopter.pMotor, 0, sizeof(Motor)*MultiCopter.num_motors);
    //配置螺旋桨模型
    Prop_SetParam(&(MultiCopter.prop), MultiCopter.mode.diameter, MultiCopter.mode.pitch, MultiCopter.mode.Ct, MultiCopter.mode.Cm, MultiCopter.mode.prop_mass);

    //初始化动力模型
    
    for (uint8_t i = 0; i < MultiCopter.num_motors; i++)
    {
        Dynamical_Init(&(MultiCopter.pMotor[i]));
        Motor_SetParam(&(MultiCopter.pMotor[i]), MultiCopter.mode.kv, MultiCopter.mode.resistance, MultiCopter.mode.esc_resistance, MultiCopter.mode.no_load_current,
                       MultiCopter.mode.pwmMin, MultiCopter.mode.pwmMax, MultiCopter.mode.spin_min, MultiCopter.mode.spin_max);
        
    }
    //return;
}


float Frame_GetAirDensity(float alt_amsl)
{
    return 0.1f;
}

void Frame_CountFM(Sim_In* input, float dt)
{

    float air_density = get_air_density(AirCraft_GetRefAlt(&(MultiCopter.air_craft)), MultiCopter.mode.TempC);
    float thrust = 0.0f;
    float moment_roll = 0.0f;
    float moment_pitch = 0.0f;
    float moment_yaw = 0.0f;    //输出总拉力

    for (uint8_t cnt = 0; cnt < MultiCopter.num_motors; cnt++)
    {
        float  force, torque;
        float pwm = input->constrols[(MultiCopter.motor_frame[cnt]._servo)];

        //根据PWM输入 计算电压
        //float command = Motor_PwmToCommand(&(MultiCopter.pMotor[cnt]), pwm);
        float command = pwm;
        //float dropped_voltage = Batterty_GetVoltage(&(MultiCopter.battery));
        float dropped_voltage = 12.6f;
        float voltage = command * dropped_voltage;

        //更新拉力和反扭矩
        Dynamical_Calculate_FM(&(MultiCopter.pMotor[cnt]), &(MultiCopter.prop),
                               voltage, air_density, dt);

        force  = Motor_GetFoce(&(MultiCopter.pMotor[cnt]));
        torque = Motor_GetTorque(&(MultiCopter.pMotor[cnt]));

        moment_roll  += force * (- sinf(Deg2Rad(MultiCopter.motor_frame[cnt]._angle))) * (MultiCopter.mode.uavR);
        moment_pitch += force * cosf(Deg2Rad(MultiCopter.motor_frame[cnt]._angle)) * (MultiCopter.mode.uavR);
        moment_yaw   += torque * (MultiCopter.motor_frame[cnt]._yaw_factor);
        thrust += force;
    }

    vec3_set(&(MultiCopter.Force), 0.0f, 0.0f, - thrust);
    vec3_set(&(MultiCopter.Torque), moment_roll, moment_pitch, moment_yaw);
}

//空气动力模型
static void AeroDynamic(Vector3f_t* force, Vector3f_t* moment, Vector3f_t* vb, Vector3f_t* wb)
{
    //空气阻力
    force->x = - 0.5f * MultiCopter.mode.Damp_Cd * (vb->x) * fabsf(vb->x);
    force->y = - 0.5f * MultiCopter.mode.Damp_Cd * (vb->y) * fabsf(vb->y);
    force->z = - 0.5f * MultiCopter.mode.Damp_Cd * (vb->z) * fabsf(vb->z);

    //空气阻力矩
    moment->x = - (MultiCopter.mode.Damp_CCm.x) * (wb->x) * fabsf(wb->x);
    moment->y = - (MultiCopter.mode.Damp_CCm.y) * (wb->y) * fabsf(wb->y);
    moment->z = - (MultiCopter.mode.Damp_CCm.z) * (wb->z) * fabsf(wb->z);
}

//陀螺力矩
static void GyroTorque(Vector3f_t* Ga, Vector3f_t* wb)
{
    for (uint8_t cnt = 0; cnt < MultiCopter.num_motors; cnt++)
    {
        Ga->x += MultiCopter.mode.intertia * (wb->y) * (MultiCopter.pMotor[cnt].rpm) * RPM_TO_RPS * MultiCopter.motor_frame[cnt]._yaw_factor;
        Ga->y -= MultiCopter.mode.intertia * (wb->x) * (MultiCopter.pMotor[cnt].rpm) * RPM_TO_RPS * MultiCopter.motor_frame[cnt]._yaw_factor;
    }
}

static double Fdm_htond(double x)
{
    int *p = (int*)&x;
    int tmp = p[0];
    p[0] = htonl(p[1]);
    p[1] = htonl(tmp);

    return x;
}

static float Fdm_htonf(float x)
{
    int* p = (int *)&x;
    *p = htonl(*p);
    return x;
}

void Fdm_packed(char* fdm_buff, int* len)
{
    FDM* fdm = NULL;
    fdm = (FDM*)(fdm_buff);
    memset(fdm_buff, 0, sizeof(FDM));
    FlyState* pState = AirCraft_GetState(&(MultiCopter.air_craft));

    fdm->version = htonl(FG_NET_FDM_VERSION);
    fdm->latitude = Fdm_htond(Deg2Rad((MultiCopter.air_craft.location.lat)*1.0e-7));
    fdm->longitude = Fdm_htond(Deg2Rad((MultiCopter.air_craft.location.lng)*1.0e-7));
    fdm->altitude = Fdm_htond(MultiCopter.air_craft.location.alt * 0.1);

    fdm->phi = Fdm_htonf(pState->euler.roll);
    fdm->theta = Fdm_htonf(pState->euler.pitch);
    fdm->psi = Fdm_htonf(pState->euler.yaw);

    fdm->elevator = Fdm_htonf(1.0f);
    fdm->num_engines = htonl(4);
    fdm->rpm[0] = Fdm_htonf(-(MultiCopter.pMotor[0].rpm));
    fdm->rpm[1] = Fdm_htonf(-(MultiCopter.pMotor[1].rpm));
    fdm->rpm[2] = Fdm_htonf(MultiCopter.pMotor[2].rpm);
    fdm->rpm[3] = Fdm_htonf(MultiCopter.pMotor[3].rpm);
    
    *len = sizeof(FDM);
}

//力和力矩->加速度和角加速度
static void Frame_FMToAcc(Vector3f_t* body_acc, Vector3f_t* rot_acc)
{
    Vector3f_t force;    
    //加上地面模型
    //Vec3_Add(&force, &(MultiCopter.Force), Land_GetForce(&(MultiCopter.land)));
    //Vec3_Copy(&force, &(MultiCopter.Force));
    force = MultiCopter.Force;
    //加上阻力
    
    FlyState* pState = AirCraft_GetState(&(MultiCopter.air_craft));
    
    Vector3f_t force_aero, moment_aero;
    //未加风干扰
    AeroDynamic(&force_aero, &moment_aero, &(pState->velocity_bf), &(pState->gyro));
    vec3_add(&force, &force, &force_aero);
    
    //加上重力
    Vector3f_t Mg_e, Mg_b;
    vec3_set(&Mg_e, 0.0f, 0.0f, MultiCopter.mode.mass * GRAVITY_MSS);
    quat_inv_vec_rotate(AirCraft_GetQuat(&(MultiCopter.air_craft)), &Mg_b, &Mg_e);
    vec3_add(&force, &force, &Mg_b);
    //考虑地面模型    
    
    Vector3f_t force_e;
    Quat_t q = AirCraft_GetQuat_q(&(MultiCopter.air_craft));
     
    quat_invert(&q);
    quat_inv_vec_rotate(&q, &force_e, &force);
    
    float alt_above = AirCraft_GetAltAboveGround(&(MultiCopter.air_craft));
    Land_Run(&(MultiCopter.land), &(pState->velocity_ef), &force_e, &(pState->gyro), alt_above, &(pState->quat), MultiCopter.mode.mass * GRAVITY_MSS);
    Vector3f_t LandForce;
    quat_inv_vec_rotate(AirCraft_GetQuat(&(MultiCopter.air_craft)), &LandForce, Land_GetForce(&(MultiCopter.land)));

    vec3_add(&force, &force, &LandForce);
    vec3_mult(body_acc, &force, 1.0f / (MultiCopter.mode.mass));

    Vector3f_t moment;
    vec3_add(&moment, &(MultiCopter.Torque), Land_GetMoment(&(MultiCopter.land)));
    

    //加上阻力矩
    vec3_add(&moment, &moment, &moment_aero);
    //加上陀螺力矩
    Vector3f_t Ga;
    vec3_zero(&Ga);
    GyroTorque(&Ga, &(pState->gyro));
    vec3_add(&moment, &moment, &Ga);
    
    //计算加速度
    rot_acc->x = moment.x / (MultiCopter.mode.jxx);
    rot_acc->y = moment.y / (MultiCopter.mode.jyy);
    rot_acc->z = moment.z / (MultiCopter.mode.jzz);
}

static float get_air_density(float alt, float tempc)
{
    float sigma, delta, theta;
    SimpleAtmosphere(alt * 0.001f, &sigma, &delta, &theta);

    const float air_pressure = SSL_AIR_PRESSURE * delta;
    return air_pressure / (ISA_GAS_CONSTANT * (C_TO_KELVIN + tempc));
}

//高度单位为km
/*
void SimpleAtmosphere(const float alt, float* sigma, float* delta, float* theta)
{
    const float GMR = 34.163195f;    //理想气体常数
    const float REARTH = 6356.766f;
    float height = alt * REARTH / (alt + REARTH);

    if (height < 11.0f)
    {
        *theta = (SSL_AIR_TEMPERATURE - 6.5f * height) / SSL_AIR_TEMPERATURE;
        *delta = powf(*theta, GMR / 6.5f);
    }
    else
    {
        *theta = 216.65f / SSL_AIR_TEMPERATURE;
        *delta = 0.2233611f * expf(-GMR * (height - 11.0f) / 216.65f);
    }
    *sigma = *theta/(*delta);
}
*/
static void Multi_LandRun(void)
{
    FlyState* pState = AirCraft_GetState(&(MultiCopter.air_craft));
    float alt_above = AirCraft_GetAltAboveGround(&(MultiCopter.air_craft));

    Land_Run(&(MultiCopter.land), &(pState->velocity_ef), &(pState->acc_body), &(pState->gyro), alt_above, &(pState->quat), MultiCopter.mode.mass * GRAVITY_MSS);
}

static void Multi_UpdateState(float dt)
{
    FlyState* pState = AirCraft_GetState(&(MultiCopter.air_craft));
    Vector3f_t rot_acc, body_acc;

    Frame_FMToAcc(&body_acc, &rot_acc);
    AirCraft_UpdateDynamics(pState, &rot_acc, &body_acc, dt);
    AirCraft_UpdateLoc(&(MultiCopter.air_craft));
    AirCraft_Publish(&(MultiCopter.air_craft));
}

static void Multi_BattRun(float dt)
{
    float current = 0.0f;
    for (uint8_t i = 0; i < MultiCopter.num_motors; i++)
    {
        current += Motor_GetInCurrent(&(MultiCopter.pMotor[i]));
    }

    Batterty_SetCurrent(&(MultiCopter.battery), current, dt);
}

Frame* Multi_GetHandle(void)
{
    return &MultiCopter;
}

void Multi_PubInState(void)
{
    uitc_digital_airplane_state tIn_State;
    FlyState* pState   = AirCraft_GetState(&(MultiCopter.air_craft));

    tIn_State.time_ms  = time_millis();
    tIn_State.acc_body = pState->acc_body;
    tIn_State.euler    = pState->euler;
    tIn_State.gyro     = pState->gyro;
    tIn_State.position = pState->position;
    tIn_State.quat = pState->quat;
    tIn_State.velocity_ef = pState->velocity_ef;
    tIn_State.location = AirCraft_GetLocation(&(MultiCopter.air_craft));
    tIn_State.num_engines = 4;
    for (uint16_t cnt = 0; cnt < tIn_State.num_engines; cnt++)
    {
        tIn_State.prop_rpm[cnt] = MultiCopter.pMotor[cnt].rpm;
    }    

    //发布内部状态信息
    itc_publish(ITC_ID(digital_airplane_state), &tIn_State);
}

void Multi_RunMain(float dt)
{
    uitc_actuator_controls actuator_controls = {0};
    Sim_In sim_in = {0};
    sim_in.mode = 0;

    if (itc_copy_from_hub(ITC_ID(vehicle_actuator_controls), &actuator_controls) != 0) {
        return;
    }

    memcpy(sim_in.constrols, actuator_controls.control, NUM_ACTUATOR_OUTPUTS);

    //计算力和力矩
    Frame_CountFM(&sim_in, dt);

    //更新动力学和运动学方程，发布状态信息
    Multi_UpdateState(dt);

    //更新电池电量
    Multi_BattRun(dt);
}

/*------------------------------------test------------------------------------*/

